#include "SimControlPanel.h"
#include <ControlParameters/ControlParameters.h>
#include <QFileDialog>
#include <QMessageBox>
#include <ParamHandler.hpp>
#include <leg_control_command_lcmt.hpp>
#include "ui_SimControlPanel.h"
#include "JoystickTest.h"


/*!
 * Display an error messagebox with the given text
 */
static void createErrorMessage(const std::string& text)
{
    QMessageBox mb;
    mb.setText(QString(text.c_str()));
    mb.exec();
}

/*!
 * Display control parameters in a qtable.
 */
static void updateQtableWithParameters(ControlParameters& params,
                                       QTableWidget& table)
{
    table.setRowCount((s32)params.collection._map.size());
    table.setColumnCount(2);

    s32 i = 0;
    for (auto& kv : params.collection._map)
    {
        (void)kv;
        for (s32 col = 0; col < 2; col++)
        {
            QTableWidgetItem* cell = table.item(i, col);
            if (!cell)
            {
                cell = new QTableWidgetItem;
                table.setItem(i, col, cell);
            }
        }

        table.item(i, 0)->setText(QString(kv.first.c_str()));
        table.item(i, 1)->setText(QString(kv.second->toString().c_str()));
        i++;
    }
}

std::string SimControlPanel::getDefaultUserParameterFileName()
{
    std::string path = getConfigDirectoryPath() + DEFAULT_USER_FILE;
    ParamHandler paramHandler(path);
    if(!paramHandler.fileOpenedSuccessfully())
    {
        throw std::runtime_error("Could not open yaml file for default user parameter file: " + path);
    }

    std::string collectionName;
    if(!paramHandler.getString("__collection-name__", collectionName))
    {
        throw std::runtime_error("Could not find __collection-name__ parameter in default user parameter file");
    }

    if(collectionName != "user-parameter-file")
    {
        throw std::runtime_error("default user parameter file has the wrong collection name, should be user-parameter-file");
    }

    std::string fileName;

    if(!paramHandler.getString("file_name", fileName))
    {
        throw std::runtime_error("default user parameter file does not have a parameter named file_name");
    }

    return fileName;
}
/*!
 * Init sim window
 */
SimControlPanel::SimControlPanel(QWidget* parent)
    : QMainWindow(parent),
      ui(new Ui::SimControlPanel),
      _userParameters("user-parameters"),
      _terrainFileName(getConfigDirectoryPath() + DEFAULT_TERRAIN_FILE),
      _heightmapLCM(getLcmUrl(255)),
      _pointsLCM(getLcmUrl(255)),
      _indexmapLCM(getLcmUrl(255)),
      _ctrlVisionLCM(getLcmUrl(255)),
      _miniCheetahDebugLCM(getLcmUrl(255))
{  
    ui->setupUi(this); // QT setup
    updateUiEnable();  // enable/disable buttons as needed.
    updateTerrainLabel(); // display name of loaded terrain file

    // attempt to load default user settings.
    _loadedUserSettings = true;

    try
    {
        _userParameters.defineAndInitializeFromYamlFile(getConfigDirectoryPath() + getDefaultUserParameterFileName());
    }
    catch (std::runtime_error& ex)
    {
        _loadedUserSettings = false;
    }

    if(!_loadedUserSettings)
    {
        printf("[SimControlPanel] Failed to load default user settings!\n");
        throw std::runtime_error("Failed to load default user settings!");
    }
    else
    {
        // display user settings in qtable if we loaded successfully
        loadUserParameters(_userParameters);
    }

    // load simulator parameters
    printf("[SimControlPanel] Init simulator parameters...\n");
    _parameters.initializeFromYamlFile(getConfigDirectoryPath() + SIMULATOR_DEFAULT_PARAMETERS);
    if (!_parameters.isFullyInitialized())
    {
        printf("[ERROR] Simulator parameters are not fully initialized.  You forgot: \n%s\n",
               _parameters.generateUnitializedList().c_str());
        throw std::runtime_error("simulator not initialized");
    }
    else
    {
        printf("\tsim parameters are all good\n");
    }

    loadSimulationParameters(_parameters);

    _pointsLCM.subscribe("cf_pointcloud", &SimControlPanel::handlePointsLCM, this);
    _pointsLCMThread = std::thread(&SimControlPanel::pointsLCMThread, this);

    _heightmapLCM.subscribe("local_heightmap", &SimControlPanel::handleHeightmapLCM, this);
    _heightmapLCMThread = std::thread(&SimControlPanel::heightmapLCMThread, this);

    _indexmapLCM.subscribe("traversability", &SimControlPanel::handleIndexmapLCM, this);
    _indexmapLCMThread = std::thread(&SimControlPanel::indexmapLCMThread, this);

    _ctrlVisionLCM.subscribe("velocity_cmd", &SimControlPanel::handleVelocityCMDLCM, this);
    _ctrlVisionLCM.subscribe("obstacle_visual", &SimControlPanel::handleObstacleLCM, this);
    _ctrlVisionLCMThread = std::thread(&SimControlPanel::ctrlVisionLCMThread, this);

    // subscribe mc debug
    _miniCheetahDebugLCM.subscribe("leg_control_data", &SimControlPanel::handleSpiDebug, this);
    _miniCheetahDebugLCMThread = std::thread([&](){
        while (_running)
        {
            _miniCheetahDebugLCM.handle();
        }
    });
}

void SimControlPanel::handleVelocityCMDLCM(const lcm::ReceiveBuffer* rbuf, 
                                           const std::string & chan,
                                           const velocity_visual_t* msg){
    (void)rbuf;
    (void)chan;

    if(_graphicsWindow){
        for(size_t i(0); i<3; ++i){
            _graphicsWindow->_vel_cmd_dir[i] = msg->vel_cmd[i];
            _graphicsWindow->_vel_cmd_pos[i] = msg->base_position[i];
        }
        _graphicsWindow->_vel_cmd_update = true;
    }
}

void SimControlPanel::handleObstacleLCM(const lcm::ReceiveBuffer* rbuf, 
                                        const std::string & chan,
                                        const obstacle_visual_t* msg){
    (void)rbuf;
    (void)chan;

    if(_graphicsWindow){
        _graphicsWindow->_obs_list.clear();
        Vec3<double> obs_loc;
        for(int i(0); i<msg->num_obs; ++i){
            obs_loc[0] = msg->location[i][0];
            obs_loc[1] = msg->location[i][1];
            obs_loc[2] = msg->location[i][2];

            _graphicsWindow->_obs_list.push_back(obs_loc);
        }
        _graphicsWindow->_obs_sigma = msg->sigma;
        _graphicsWindow->_obs_height = msg->height;

        _graphicsWindow->_obstacle_update = true;
        //printf("%f, %f\n", _graphicsWindow->_obs_list[0][0], _graphicsWindow->_obs_list[0][1]);
    }

}
void SimControlPanel::handlePointsLCM(
        const lcm::ReceiveBuffer *rbuf,
        const std::string &chan,
        const rs_pointcloud_t*msg)
{
    (void)rbuf;
    (void)chan;

    if(_graphicsWindow)
    {
        for(size_t i(0); i<_graphicsWindow->_num_points; ++i)
        {
            for(size_t j(0); j<3; ++j)
            {
                _graphicsWindow->_points[i][j] = msg->pointlist[i][j];
            }
        }

        _graphicsWindow->_pointcloud_data_update = true;
    }
}

void SimControlPanel::handleIndexmapLCM(const lcm::ReceiveBuffer *rbuf,
                                        const std::string &chan,
                                        const traversability_map_t *msg)
{
    (void)rbuf;
    (void)chan;

    if(_graphicsWindow)
    {
        for(size_t i(0); i<_graphicsWindow->x_size; ++i)
        {
            for(size_t j(0); j<_graphicsWindow->y_size; ++j)
            {
                _graphicsWindow->_idx_map(i,j) = msg->map[i][j];
            }
        }
        _graphicsWindow->_indexmap_data_update = true;
    }
}


void SimControlPanel::handleHeightmapLCM(const lcm::ReceiveBuffer *rbuf,
                                         const std::string &chan,
                                         const heightmap_t *msg) {
    (void)rbuf;
    (void)chan;

    if(_graphicsWindow){
        for(size_t i(0); i<_graphicsWindow->x_size; ++i){
            for(size_t j(0); j<_graphicsWindow->y_size; ++j){
                _graphicsWindow->_map(i,j) = msg->map[i][j];
            }
        }
        _graphicsWindow->_pos[0] = msg->robot_loc[0];
        _graphicsWindow->_pos[1] = msg->robot_loc[1];
        _graphicsWindow->_pos[2] = msg->robot_loc[2];

        _graphicsWindow->_heightmap_data_update = true;
    }
}

void SimControlPanel::handleSpiDebug(const lcm::ReceiveBuffer *rbuf, const std::string &chan,
                                     const leg_control_data_lcmt *msg)
{
    (void)rbuf;
    (void)chan;
    MiniCheetahDebugData ddata;

    u32 idx = 0;
    for(u32 leg = 0; leg < 4; leg++)
    {
        for(u32 joint = 0; joint < 3; joint++)
        {
            ddata.p[leg][joint] = msg->q[idx];
            ddata.v[leg][joint] = msg->qd[idx];
            idx++;
        }
    }

    if(_mcDebugWindow.setDebugData(ddata))
    {
        MiniCheetahDebugCommand cmd;
        _mcDebugWindow.getDebugCommand(cmd);

        leg_control_command_lcmt lcm_cmd;
        memset(&lcm_cmd, 0, sizeof(leg_control_command_lcmt));
        idx = 0;
        for(u32 leg = 0; leg < 4; leg++)
        {
            for(u32 joint = 0; joint < 3; joint++)
            {
                if(cmd.enable[leg][joint])
                {
                    lcm_cmd.q_des[idx] = cmd.qd[leg][joint];
                    lcm_cmd.kp_joint[idx] = cmd.kp[leg][joint];
                    lcm_cmd.kd_joint[idx] = cmd.kd[leg][joint];
                }
                idx++;
            }
        }

        _miniCheetahDebugLCM.publish("spi_debug_cmd", &lcm_cmd);
    }

}

void SimControlPanel::closeEvent(QCloseEvent* event)
{
    //stop_run();
    if (ui->stopButton->isEnabled())
    {
        on_stopButton_clicked();
        //stop_run();
        usleep(100);
    }
    _running = false;
    _pointsLCMThread.detach();
    _heightmapLCMThread.detach();
    _indexmapLCMThread.detach();
    _ctrlVisionLCMThread.detach();
    _miniCheetahDebugLCMThread.detach();

    usleep(100000);
    event->accept();
}

void SimControlPanel::stop_run()
{
    if (_interfaceTaskManager == nullptr)
        return;

    if (_simulation != nullptr)
    {
        _simulation->stop();
        _simThread.join();
    }
    else
    {
        _robotInterface->stopInterface();
    }

    if (_graphicsWindow != nullptr)
    {
        _graphicsWindow->setAnimating(false);
        _graphicsWindow->hide();
    }

    delete _interfaceTaskManager;
    _interfaceTaskManager = nullptr;

    delete _robotInterface;
    _robotInterface = nullptr;

    delete _simulation;
    _simulation = nullptr;

    delete _graphicsWindow;
    _graphicsWindow = nullptr;

    _state = SimulationWindowState::STOPPED;
    updateUiEnable();
}


SimControlPanel::~SimControlPanel()
{
    if (_simulation)
        delete _simulation;

    if (_interfaceTaskManager)
        delete _interfaceTaskManager;

    if (_robotInterface)
        delete _robotInterface;

    if (_graphicsWindow)
        delete _graphicsWindow;

    delete ui;
}

/*!
 * External notification of UI update needed
 */
void SimControlPanel::update_ui()
{
    updateUiEnable();
}

/*!
 * Enable/disable buttons as needed based on what is running
 */
void SimControlPanel::updateUiEnable()
{
    ui->startButton->setEnabled(!(isRunning() || isError()));
    ui->stopButton->setEnabled(isRunning() || isError());
    ui->joystickButton->setEnabled(isRunning() || isError());
    ui->robotTable->setEnabled(isRunning() || isError());
    ui->goHomeButton->setEnabled(isRunning() || isError());

    switch(_state)
    {
    case SimulationWindowState::STOPPED:
        ui->simulatorStateLabel->setText("Simulator State: Stopped");
        ui->stopButton->setText("Stop");
        break;

    case SimulationWindowState::RUNNING:
        ui->simulatorStateLabel->setText("Simulator State: Running");
        ui->stopButton->setText("Stop");
        break;

    case SimulationWindowState::ERROR:
        ui->simulatorStateLabel->setText("Simulator State: Error");
        ui->stopButton->setText("Reset Error");
        break;
    }

    if((isRunning() || isError()) && _simulation)
    {
        if(_simulation->isRobotConnected())
        {
            ui->simulatorConnectedLabel->setText("Sim Connection: Yes");
        }
        else
        {
            ui->simulatorConnectedLabel->setText("Sim Connection: No");
        }
    }
    else
    {
        ui->simulatorConnectedLabel->setText("Sim Connection: N/A");
    }
}

/*!
 * Update the name of the loaded terrain file label
 */
void SimControlPanel::updateTerrainLabel()
{
    ui->terrainFileLabel->setText(QString(_terrainFileName.c_str()));
}

/*!
 * Simulation error
 */
void SimControlPanel::errorCallback(std::string errorMessage)
{
    _state = SimulationWindowState::ERROR; // go to error state
    updateUiEnable(); // update UI
    createErrorMessage("Simulation Error\n" + errorMessage); // display error dialog
}

/*!
 * Start a simulation/robot run
 */
void SimControlPanel::on_startButton_clicked()
{
    // get robot type
    RobotType robotType;

    if (ui->cheetah3Button->isChecked())
    {
        robotType = RobotType::CHEETAH_3;
    }
    else if (ui->miniCheetahButton->isChecked())
    {
        robotType = RobotType::MINI_CHEETAH;
    }
    else
    {
        createErrorMessage("Error: you must select a robot");
        return;
    }

    // get run type
    if (!ui->simulatorButton->isChecked() && !ui->robotButton->isChecked())
    {
        createErrorMessage("Error: you must select either robot or simulation mode");
        return;
    }

    _simulationMode = ui->simulatorButton->isChecked();

    // graphics
    printf("[SimControlPanel] Initialize Graphics...\n");
    _graphicsWindow = new Graphics3D();
    _graphicsWindow->show();
    _graphicsWindow->resize(1280, 720);

    if (_simulationMode)
    {
        // run a simulation
        try
        {
            printf("[SimControlPanel] Initialize simulator...\n");
            _simulation = new Simulation(robotType, _graphicsWindow, _parameters, _userParameters,
                                         // this will allow the simulation thread to poke us when there's a state change
                                         [this]()
            {
                QMetaObject::invokeMethod(this,"update_ui");
            });

            loadSimulationParameters(_simulation->getSimParams());
            loadRobotParameters(_simulation->getRobotParams());

            // terrain
            printf("[SimControlParameter] Load terrain...\n");
            _simulation->loadTerrainFile(_terrainFileName);

        } catch (std::exception& e)
        {
            createErrorMessage("FATAL: Exception thrown during simulator setup\n" + std::string(e.what()));
            throw e;
        }

        // start sim
        _simThread = std::thread(
                    // simulation function
                    [this]()
        {
            // error callback function
            std::function<void(std::string)> error_function = [this](std::string str)
            {
                // Qt will take care of doing the call in the UI event loop
                QMetaObject::invokeMethod(this, [=]()
                {
                    this->errorCallback(str);
                });
            };

            try
            {
                // pass error callback to simulator
                _simulation->runAtSpeed(error_function);
            } catch (std::exception &e)
            {
                // also catch exceptions
                error_function("Exception thrown in simulation thread: " + std::string(e.what()));
            }
        });

        // graphics start
        _graphicsWindow->setAnimating(true);
    }
    else
    {
        printf("[SimControlPanel] Init Robot Interface...\n");
        _interfaceTaskManager = new PeriodicTaskManager;
        _robotInterface = new RobotInterface(robotType,
                                             _graphicsWindow,
                                             _interfaceTaskManager,
                                             _userParameters);
        loadRobotParameters(_robotInterface->getParams());
        _robotInterface->startInterface();
        _graphicsWindow->setAnimating(true);
    }

    _state = SimulationWindowState::RUNNING;
    updateUiEnable();
}

/*!
 * Stop the currently running simulation or robot connection
 */
void SimControlPanel::on_stopButton_clicked() {
    if (_simulation) {
        _simulation->stop();
        _simThread.join();
    } else {
        _robotInterface->stopInterface();
    }

    if (_graphicsWindow) {
        _graphicsWindow->setAnimating(false);
        _graphicsWindow->hide();
    }

    delete _interfaceTaskManager;
    delete _robotInterface;
    delete _simulation;
    delete _graphicsWindow;

    _simulation = nullptr;
    _graphicsWindow = nullptr;
    _robotInterface = nullptr;
    _interfaceTaskManager = nullptr;

    _state = SimulationWindowState::STOPPED;
    updateUiEnable();
}



/*!
 * Populate the simulator qtable parameters
 */
void SimControlPanel::loadSimulationParameters(
        SimulatorControlParameters& params) {
    _ignoreTableCallbacks = true;
    updateQtableWithParameters(params, *ui->simulatorTable);
    _ignoreTableCallbacks = false;
}

/*!
 * Populate the robot qtable parameters
 */
void SimControlPanel::loadRobotParameters(RobotControlParameters& params) {
    _ignoreTableCallbacks = true;
    updateQtableWithParameters(params, *ui->robotTable);
    _ignoreTableCallbacks = false;
}

/*!
 * Populate the robot qtable parameters
 */
void SimControlPanel::loadUserParameters(ControlParameters& params) {
    _ignoreTableCallbacks = true;
    updateQtableWithParameters(params, *ui->userControlTable);
    _ignoreTableCallbacks = false;
}

/*!
 * Attempt to reset the joystick if a new one is connected
 */
void SimControlPanel::on_joystickButton_clicked() {
    if(isRunning()) {
        _graphicsWindow->resetGameController();
        JoystickTestWindow* window = new JoystickTestWindow(_graphicsWindow->getGameController());
        window->exec();
        delete window;
    }
}

void SimControlPanel::on_driverButton_clicked() {
    _mcDebugWindow.show();
}

/*!
 * Respond to a change in the simulator table.
 */
void SimControlPanel::on_simulatorTable_cellChanged(int row, int column) {
    if (_ignoreTableCallbacks) return;

    // we only allow values to change, which are in column 1
    if (column != 1) {
        return;
    }

    // get the name of the parameter....
    auto cell = ui->simulatorTable->item(row, 0);
    std::string cellName = cell->text().toStdString();

    if (cellName == "") {
        return;
    }

    // get the parameters
    auto& parameter = _parameters.collection.lookup(cellName);
    ControlParameterValueKind kind = parameter._kind;
    ControlParameterValue oldValue = parameter.get(kind);

    bool success = true;

    // attempt to set, based on string.
    try {
        parameter.setFromString(
                    ui->simulatorTable->item(row, 1)->text().toStdString());
    } catch (std::exception& e) {
        success = false;
    }

    // if it fails (bad user input string), restore to the old value
    if (!success) {
        printf("[ERROR] invalid data, restoring old data!\n");
        // set parameter value
        parameter.set(oldValue, kind);

        assert(!_ignoreTableCallbacks);

        // manually fix the table
        _ignoreTableCallbacks = true;
        ui->simulatorTable->item(row, 1)->setText(
                    QString(_parameters.collection.lookup(cellName).toString().c_str()));
        _ignoreTableCallbacks = false;
    } else {
        // this update "rewrites" the value in the table.  If it's an integer, it kills any
        // decimal.  If it's a float, it puts in scientific notation if needed.
        _ignoreTableCallbacks = true;
        ui->simulatorTable->item(row, 1)->setText(
                    QString(parameter.toString().c_str()));
        _ignoreTableCallbacks = false;
    }
}

/*!
 * Save simulation config to file
 */
void SimControlPanel::on_saveSimulatorButton_clicked() {
    QString fileName = QFileDialog::getSaveFileName(
                nullptr, ("Save Simulator Table Values"), "../config", "All Files (*)");
    if (fileName == nullptr || fileName == "") {
        createErrorMessage("File name is invalid");
        return;
    }

    _parameters.lockMutex();
    _parameters.writeToYamlFile(fileName.toStdString());
    _parameters.unlockMutex();
}

/*!
 * Load simulation config from file
 */
void SimControlPanel::on_loadSimulatorButton_clicked() {
    QString fileName = QFileDialog::getOpenFileName(
                nullptr, ("Load Simulator Table Values"), "../config", "All Files (*)");
    if (fileName == nullptr || fileName == "") {
        createErrorMessage("File name is invalid");
        return;
    };
    _parameters.collection.clearAllSet();
    _parameters.initializeFromYamlFile(fileName.toStdString());
    if (!_parameters.collection.checkIfAllSet()) {
        printf(
                    "new settings file %s doesn't contain the following simulator "
                    "parameters:\n%s\n",
                    fileName.toStdString().c_str(),
                    _parameters.generateUnitializedList().c_str());
        throw std::runtime_error("bad new settings file");
    }
    loadSimulationParameters(_parameters);
    _parameters.unlockMutex();
}

void SimControlPanel::on_robotTable_cellChanged(int row, int column) {
    if (_ignoreTableCallbacks) return;
    if (column != 1) {
        return;
    }

    auto cell = ui->robotTable->item(row, 0);
    std::string cellName = cell->text().toStdString();

    if (cellName == "") {
        return;
    }

    auto& parameter = (_simulationMode ? _simulation->getRobotParams()
                                       : _robotInterface->getParams())
            .collection.lookup(cellName);
    ControlParameterValueKind kind = parameter._kind;
    ControlParameterValue oldValue = parameter.get(kind);

    bool success = true;

    try {
        parameter.setFromString(ui->robotTable->item(row, 1)->text().toStdString());
    } catch (std::exception& e) {
        success = false;
    }

    if (!success) {
        printf("[ERROR] invalid data, restoring old data!\n");
        parameter.set(oldValue, kind);

        assert(!_ignoreTableCallbacks);

        _ignoreTableCallbacks = true;
        ui->robotTable->item(row, 1)->setText(
                    QString(parameter
                            .toString()
                            .c_str()));
        _ignoreTableCallbacks = false;
    } else {
        if (_simulationMode) {
            if (_simulation->isRobotConnected()) {
                _simulation->sendControlParameter(
                            cellName, parameter.get(parameter._kind), parameter._kind, false);
            }

            _ignoreTableCallbacks = true;
            ui->robotTable->item(row, 1)->setText(
                        QString(parameter.toString().c_str()));
            _ignoreTableCallbacks = false;
        } else {
            _robotInterface->sendControlParameter(
                        cellName, parameter.get(parameter._kind), parameter._kind, false);
        }
    }
}

void SimControlPanel::on_saveRobotButton_clicked() {
    QString fileName = QFileDialog::getSaveFileName(
                nullptr, ("Save Robot Table Values"), "../config", "All Files (*)");
    if (fileName == nullptr || fileName == "") {
        createErrorMessage("File name is invalid");
        return;
    }
    _simulation->getRobotParams().writeToYamlFile(fileName.toStdString());
}

void SimControlPanel::on_loadRobotButton_clicked() {
    QString fileName = QFileDialog::getOpenFileName(
                nullptr, ("Load Quadruped Table Values"), "../config", "All Files (*)");
    if (fileName == nullptr || fileName == "") {
        createErrorMessage("File name is invalid");
        return;
    }

    if (_simulationMode) {
        _simulation->getRobotParams().lockMutex();
        _simulation->getRobotParams().collection.clearAllSet();
        _simulation->getRobotParams().initializeFromYamlFile(
                    fileName.toStdString());
        if (!_simulation->getRobotParams().collection.checkIfAllSet()) {
            printf(
                        "new settings file %s doesn't contain the following robot "
                        "parameters:\n%s\n",
                        fileName.toStdString().c_str(),
                        _simulation->getRobotParams().generateUnitializedList().c_str());
            throw std::runtime_error("bad new settings file");
        }
        loadRobotParameters(_simulation->getRobotParams());

        if (_simulation->isRobotConnected()) {
            for (auto& kv : _simulation->getRobotParams().collection._map) {
                _simulation->sendControlParameter(
                            kv.first, kv.second->get(kv.second->_kind), kv.second->_kind, false);
            }
        }
        _simulation->getRobotParams().unlockMutex();
    } else {
        _robotInterface->getParams().lockMutex();
        _robotInterface->getParams().collection.clearAllSet();
        _robotInterface->getParams().initializeFromYamlFile(fileName.toStdString());
        if (!_robotInterface->getParams().collection.checkIfAllSet()) {
            printf(
                        "new settings file %s doesn't contain the following robot "
                        "parameters:\n%s\n",
                        fileName.toStdString().c_str(),
                        _robotInterface->getParams().generateUnitializedList().c_str());
            throw std::runtime_error("bad new settings file");
        }
        loadRobotParameters(_robotInterface->getParams());

        for (auto& kv : _robotInterface->getParams().collection._map) {
            _robotInterface->sendControlParameter(
                        kv.first, kv.second->get(kv.second->_kind), kv.second->_kind, false);
        }

        _robotInterface->getParams().unlockMutex();
    }
}

void SimControlPanel::on_setTerrainButton_clicked() {
    QString fileName = QFileDialog::getOpenFileName(
                nullptr, ("Load Terrain Definition"), "../config", "All Files (*)");
    if (fileName == nullptr || fileName == "") {
        createErrorMessage("File name is invalid");
        return;
    }

    _terrainFileName = fileName.toStdString();
    updateTerrainLabel();
}

void SimControlPanel::on_userControlTable_cellChanged(int row, int column) {
    if (_ignoreTableCallbacks) return;
    if (column != 1) {
        return;
    }

    auto cell = ui->userControlTable->item(row, 0);
    std::string cellName = cell->text().toStdString();

    if (cellName == "") {
        return;
    }

    auto& parameter = _userParameters.collection.lookup(cellName);
    //  auto& parameter = (_simulationMode ? _simulation->getRobotParams()
    //                                     : _robotInterface->getParams())
    //      .collection.lookup(cellName);
    ControlParameterValueKind kind = parameter._kind;
    ControlParameterValue oldValue = parameter.get(kind);

    bool success = true;

    try {
        parameter.setFromString(ui->userControlTable->item(row, 1)->text().toStdString());
    } catch (std::exception& e) {
        success = false;
    }

    if (!success) {
        printf("[ERROR] invalid data, restoring old data!\n");
        parameter.set(oldValue, kind);

        assert(!_ignoreTableCallbacks);

        _ignoreTableCallbacks = true;
        ui->userControlTable->item(row, 1)->setText(
                    QString(_userParameters
                            .collection.lookup(cellName)
                            .toString()
                            .c_str()));
        _ignoreTableCallbacks = false;
    } else {
        if(isRunning() || isError()) {
            if (_simulationMode) {
                if (_simulation && _simulation->isRobotConnected()) {
                    _simulation->sendControlParameter(
                                cellName, parameter.get(parameter._kind), parameter._kind, true);
                }

                _ignoreTableCallbacks = true;
                ui->userControlTable->item(row, 1)->setText(
                            QString(parameter.toString().c_str()));
                _ignoreTableCallbacks = false;
            } else {
                _robotInterface->sendControlParameter(
                            cellName, parameter.get(parameter._kind), parameter._kind, true);
            }
        }
    }
}

void SimControlPanel::on_loadUserButton_clicked() {
    QString fileName = QFileDialog::getOpenFileName(
                nullptr, ("Load User Table Values"), "../config", "All Files (*)");
    if (fileName == nullptr || fileName == "") {
        createErrorMessage("File name is invalid");
        return;
    }

    _userParameters.lockMutex();
    _userParameters.collection.deleteAll();
    _userParameters.defineAndInitializeFromYamlFile(
                fileName.toStdString());
    loadUserParameters(_userParameters);
    _userParameters.unlockMutex();
    _loadedUserSettings = true;

    if(isRunning() || isError()) {
        if (_simulationMode) {
            if (_simulation && _simulation->isRobotConnected()) {
                for (auto& kv : _userParameters.collection._map) {
                    _simulation->sendControlParameter(
                                kv.first, kv.second->get(kv.second->_kind), kv.second->_kind, true);
                }
            }
        } else {
            for (auto& kv : _userParameters.collection._map) {
                _robotInterface->sendControlParameter(
                            kv.first, kv.second->get(kv.second->_kind), kv.second->_kind, true);
            }

        }
    }
}

void SimControlPanel::on_saveUserButton_clicked() {
    QString fileName = QFileDialog::getSaveFileName(
                nullptr, ("Save User Table Values"), "../config", "All Files (*)");
    if (fileName == nullptr || fileName == "") {
        createErrorMessage("File name is invalid");
        return;
    }
    _userParameters.writeToYamlFile(fileName.toStdString());
}

void SimControlPanel::on_goHomeButton_clicked() {
    printf("go home\n");
    FBModelState<double> homeState;
    homeState.bodyOrientation << 1, 0, 0, 0;
    homeState.bodyPosition = Vec3<double>(0, 0, 0.4);
    homeState.bodyVelocity = SVec<double>::Zero();
    homeState.q = DVec<double>(12);
    homeState.q << -0.05, -0.8, 1.7, 0.05, -0.8, 1.7, -0.05, -0.8, 1.7, 0.05, -0.8, 1.7;
    homeState.qd = homeState.q;

    _simulation->setRobotState(homeState);
}

void SimControlPanel::on_kickButton_clicked() {
    // velocity of the floating base:
    SVec<double> kickVelocity;
    kickVelocity << ui->kickAngularX->text().toDouble(),
            ui->kickAngularY->text().toDouble(), ui->kickAngularZ->text().toDouble(),
            ui->kickLinearX->text().toDouble(), ui->kickLinearY->text().toDouble(),
            ui->kickLinearZ->text().toDouble();

    FBModelState<double> state = _simulation->getRobotState();
    state.bodyVelocity += kickVelocity;
    _simulation->setRobotState(state);
}
